/******************************************************************************
** 
** File name:	rtos_message.S
**	
** Description:	Basic RTOS message management. Implementation of the 
**				RTOS_SendMessage and RTOS_WaitMessage functions and all 
**				inherited services.
**
** RTOS ver:	3.0
**
** Modules/Functions:
**
**	void 			RTOS_SendMessage( RTOS_MSQ_t * msq, RTOS_MSG_t * msg )
**	void 			RTOS_SendMessageISR(RTOS_MSQ_t * msq , RTOS_MSG_t * msg)
**	RTOS_MSG_t * 	RTOS_WaitMessage( RTOS_MSQ_t * msq)
**	RTOS_MSG_t * 	RTOS_RecvMessage( RTOS_MSQ_t * msq)		
**
** Notes: none
**
*******************************************************************************
** History:
**
**	10/15/2011 - AK - Moving to RTOS Ver.2. First prototype version.
** 	11/20/2011 - AK - Services had been revised. Added RTOS_RecvMessage()
**	07/26/2012 - AK - RTOS API is downgraded to comply with earliest rtos v2
**  08/08/2012 - AK - RTOS source code is checked for byte-by-byte complience 
**					with MPP binary (Dec 8, 2011).
**	09/27/2012 - AK - RTOS v3. _ENTER_CRITICAL/_LEAVE_CRITICAL 
**								 _RTOS_PROLOG/_RTOS_EPILOG
**	09/28/2012 - AK - fixed RTOS_SendMessage() service (see unit tests 52/53)
** 		
******************************************************************************/
#include        "rtos.inc"

        _MODULE(rtos_message)

        _EXTERN(_EnterISR)
        _EXTERN(_LeaveISR)
        _EXTERN(_AppendObj)
        _EXTERN(RTOS_SetObjectEventISR)
        _EXTERN(_RemoveObjTop)
        _EXTERN(RTOS_WaitObjectEvent)
        _EXTERN(RTOS_ResetEvent)
        _EXTERN(RTOS_ResetObjectEvent)

        _CODESECTION
		
/******************************************************************************
;; Prototype:	void RTOS_SendMessage( RTOS_MSQ_t * msq, RTOS_MSG_t * msg )
;; Description:	Sending message to the specified message queue	
;; Passed:		p1h:p1l = &msq - Message queue head	
;;				p2h:p2l = &msg - Message to send
;; Returns:		-	
;; Uses:		-
;; Stack:		one context is saved to the task' stack	
;; Calls:		_RTOS_PROLOG/_RTOS_EPILOG; RTOS_SendMessageISR	
;; Assumes:		-	
;; Notes:		can be called only from task level	
******************************************************************************/
	_FUNCTION(RTOS_SendMessage)
        
RTOS_SendMessage:

    ;;rcall 	_EnterISR
	
	_ENTER_CRITICAL						;; Begin critical section (all interrupts disabled)
	
	_RTOS_PROLOG						;; Enter system level
	
    rcall   RTOS_SendMessageISR			;; Safe to use ISR service as we are on system level
										;; TBD: check if it's better to put code here instead
										;; of calling RTOS_SendMessageISR
	;; rjmp    _LeaveISR
	
	_ENTER_CRITICAL						;; Enter critical again as previous service terminates 
										;; it on return
	
	_RTOS_EPILOG						;; leave system level and terminate critical section
	
    _ENDFUNC 	;; _FUNCTION(RTOS_SendMessage)

/******************************************************************************
** Prototype:	void RTOS_SendMessageISR(RTOS_MSQ_t * msq , RTOS_MSG_t * msg)
** Description:		
** Passed:		p1h:p1l = &msq - Message queue head	
**				p2h:p2l = &msg - Message to send
** Returns:			
** Uses:		Z, Y & some others.  See RTOS_SetObjectEventISR	
** Stack:			
** Calls:		RTOS_SetObjectEventISR	
** Assumes:		Called within an _LeaveISR/_EnterISR wrapper	
** Notes:		This could be called from interrupt code
**              but you would need to make sure there is 
**              only one source of messages for that queue
**              or wrap _AppendObj in a critical section.	
******************************************************************************/
		
	_FUNCTION(RTOS_SendMessageISR)
        
RTOS_SendMessageISR:

	;; rcall   RTOS_ResetObjectEvent

	mov     Zh, p1h
	mov     Zl, p1l
	
	_ENTER_CRITICAL						;; Enter critical section (disable all interrupts)
										;; So it safe to access the queue from various interrupts
	rcall   _AppendObj   				;; Append the message onto the queue
	
	_LEAVE_CRITICAL						;; Leave critical section (enable upper level interrupts)
	
	rjmp    RTOS_SetObjectEventISR		
	
	_ENDFUNC 	;; _FUNCTION(RTOS_SendMessageISR)

/******************************************************************************
;; Prototype:	RTOS_MSG_t * RTOS_WaitMessage( RTOS_MSQ_t * msq)
;; Description:	wait for some message to be received by specified message queue	
;; Passed:		p1h:p1l = &msq - Message queue head		
;; Returns:		r1h:r1l = &msg - Message to send	
;; Uses:		Z, flags (See _RemoveObjTop)	
;; Stack:		TBD
;; Calls:		_RemoveObjTop; RTOS_WaitObjectEvent; RTOS_ResetObjectEvent
;; Assumes:		-	
;; Notes:		RTOS_RecvMessage is atomic for append-only queues
;;              just push registers to be saved	
******************************************************************************/
    _FUNCTION(RTOS_WaitMessage)

RTOS_WaitMessage:

	mov     Zl, p1l
	mov     Zh, p1h

	_ENTER_CRITICAL						;; Begin critical section (disable all interrupts)
			
	rcall   _RemoveObjTop
	
	brne    _rm01

	rcall   RTOS_WaitObjectEvent

	subi    p1l, lo8(2)
	sbci    p1h, hi8(2)           		;; p1 buggered by WaitObjectEvent
	
	rjmp    RTOS_WaitMessage
	
_rm01:

	rcall   RTOS_ResetObjectEvent   	;; Clear possible _PEND
										;; and leave critical section
	mov     p1l, p2l
	mov     p1h, p2h

	;; _LEAVE_CRITICAL					;; not required here ( leave critical is made 
										;; inside RTOS_ResetObjectEvent )
	
	ret
		
    _ENDFUNC	;; _FUNCTION(RTOS_WaitMessage)


/******************************************************************************
;; Prototype:	RTOS_MSG_t * RTOS_RecvMessage( RTOS_MSQ_t * msq)
;; Description:	Removes the first item in a queue. Return Zero if empty.
;; Passed:		p1h:p1l = &msq - Message queue head		
;; Returns:		r1h:r1l = &msg - Message received (0 if empty)	
;; Uses:		Z, X, Flags, R23:R22	
;; Stack:		TBD
;; Calls:		_RemoveObjTop; RTOS_ResetEvent
;; Assumes:		-	
;; Notes:		If queue empty, then reset queue event	
******************************************************************************/
    _FUNCTION(RTOS_RecvMessage)
        
RTOS_RecvMessage:

	mov     Zl, p1l
	mov     Zh, p1h

	_ENTER_CRITICAL					;; Begin critical section (disable all interrupts)
	
	rcall   _RemoveObjTop			;; It's safe to remove message from the queue
	
	_LEAVE_CRITICAL					;; Leave critical
	
	subi    p1l, lo8(-2)
	sbci    p1h, hi8(-2)
	
	rcall   RTOS_ResetEvent      	;; Reset message' event and leave critical section
	
	mov     r1l, p2l
	mov     r1h, p2h
		
	;;	_LEAVE_CRITICAL				;; Not required here as the critical section 
									;; will be ended by RTOS_ResetEevent
	
	ret

    _ENDFUNC	;; _FUNCTION(RTOS_RecvMessage)

	
        _END	;;  _MODULE(rtos_message)

